package WayPoints;



import java.util.ArrayList;

import Movement.NERoom;

import lejos.nxt.LCD;
import lejos.robotics.navigation.MoveController;
import lejos.robotics.navigation.NavPathController;
import lejos.robotics.navigation.WayPoint;


public class NEBestPath {
   
    //Pièce actuelle ou se trouve le robot
    protected NERoom currentRoom;
   
    //destination du robot
    protected NERoom DestinationRoom;
   
   
    //condtructeur
    public NEBestPath(NERoom pieceActuel, NERoom pieceDestination)
    {
        this.currentRoom=pieceActuel;
        this.DestinationRoom=pieceDestination;
    }
   
   
    //ajoute un point dans le chemin actuel
    public ArrayList<WayPoint> addWayPoints(ArrayList<WayPoint> listPath,ArrayList<WayPoint> bestPath,int indiceStartCopy)
    {
        //for(int i = indiceStartCopy + 1; i < listPath.size();i++)
        for(int i = indiceStartCopy; i < listPath.size();i++)
            {
                bestPath.add(listPath.get(i));
            }
        return bestPath;
    }
   
   
    // Calcul de l'intersection entre deux droites
    // Ordre des arguments : points A et B de la droite 1, puis A et B de la droite 2
    public ArrayList<Double> computeIntersection(WayPoint pA1, WayPoint pB1, WayPoint pA2, WayPoint pB2)
    {
        ArrayList<Double> result = new ArrayList<Double>();
       
        // Calcul de la premi�re droite a1 * x + b1
        if (pB1.getX() != pA1.getX() && (pB2.getX() != pA2.getX())) // si les droites 1 et 2 ne sont pas verticales
        {
            double a1 = (pB1.getY() - pA1.getY()) / (pB1.getX() - pA1.getX());
            double b1 = pB1.getY() - a1 * pB1.getX();
           
            // Calcul de la deuxi�me droite a2 * x + b2
            double a2 = (pB2.getY() - pA2.getY()) / (pB2.getX() - pA2.getX());
            double b2 = pB2.getY() - a2 * pB2.getX();
                       
            // Calcul de l'intersection si les droites ne sont pas parrall�les
            if (a1 != a2)
            {
                double X = (b2 - b1) / (a1 - a2);
                double Y = (a1 * b2 - b1 * a2) / (a1 - a2);
               
                // On v�rifie que le point d'intersection est dans le segment [pA1 pB1]
                if (!((X <= pA1.getX() && X <= pB1.getX()) ||
                      (X >= pA1.getX() && X >= pB1.getX()) ||
                      (Y <= pA1.getY() && Y <= pB1.getY()) ||
                      (Y >= pA1.getY() && Y >= pB1.getY())))
                {
                    // On v�rifie que le point d'intersection est dans le segment [pA2 pB2]
                    if (!((X <= pA2.getX() && X <= pB2.getX()) ||
                              (X >= pA2.getX() && X >= pB2.getX()) ||
                              (Y <= pA2.getY() && Y <= pB2.getY()) ||
                              (Y >= pA2.getY() && Y >= pB2.getY())))
                    {
                        result.add(X);
                        result.add(Y);
                    }
                }
            }
            //on teste si les droites sont confondues
            else if (b1 == b2)
            {
                // On teste si le segment [pA1 pB1] est compris dans le segment [pA2 pB2]
                if (!((pA1.getX() < pA2.getX() && pA1.getX() < pB2.getX()) ||
                      (pA1.getX() > pA2.getX() && pA1.getX() > pB2.getX()) ||
                      (pA1.getY() < pA2.getY() && pA1.getY() < pB2.getY()) ||
                      (pA1.getY() > pA2.getY() && pA1.getY() > pB2.getY()))
                      &&
                    !((pB1.getX() < pA2.getX() && pB1.getX() < pB2.getX()) ||
                      (pB1.getX() > pA2.getX() && pB1.getX() > pB2.getX()) ||
                      (pB1.getY() < pA2.getY() && pB1.getY() < pB2.getY()) ||
                      (pB1.getY() > pA2.getY() && pB1.getY() > pB2.getY())))
                {
                    result.add(pB2.getX());
                    result.add(pB2.getY());
                }
            }
        }
    else if (pB1.getX() == pA1.getX() && (pB2.getX() != pA2.getX())) // Si droite 1 verticale et droite 2 pas verticale
    {
        // Calcul de la deuxi�me droite a2 * x + b2
        double a2 = (pB2.getY() - pA2.getY()) / (pB2.getX() - pA2.getX());
        double b2 = pB2.getY() - a2 * pB2.getX();
       
        double X = pB1.getX();
        double Y = a2 * X + b2;
       
        if((Y < pA1.getY() && Y > pB1.getY()) || (Y > pA1.getY() && Y < pB1.getY()))
        {
            result.add(X);
            result.add(Y);
        }
    }
    else if (pB1.getX() != pA1.getX() && (pB2.getX() == pA2.getX())) // Si droite 1 pas verticale et droite 2 verticale
    {
        double a1 = (pB1.getY() - pA1.getY()) / (pB1.getX() - pA1.getX());
        double b1 = pB1.getY() - a1 * pB1.getX();
       
        double X = pB2.getX();
        double Y = a1 * X + b1;
       
        if((Y < pA2.getY() && Y > pB2.getY()) || (Y > pA2.getY() && Y < pB2.getY()))
        {
            result.add(X);
            result.add(Y);
        }
    }
    // Si les droites 1 et 2 sont verticales
    else if (pB1.getX() == pA1.getX() && (pB2.getX() == pA2.getX()))
    {
        // Si les droites sont confondues
        if (pB1.getX() == pB2.getX())
        {
            double X = pA1.getX();
            //TODO
            if ((pB1.getY() <= pA2.getY() && pB1.getY() >= pB2.getY()) || (pB1.getY() >= pA2.getY() && pB1.getY() <= pB2.getY()))
            {
                double Y = pB2.getY();
                result.add(X);
                result.add(Y);
            }
            else if ((pA1.getY() <= pA2.getY() && pA1.getY() >= pB2.getY()) || (pA1.getY() >= pA2.getY() && pA1.getY() <= pB2.getY()))
            {
                double Y = pA1.getY();
                result.add(X);
                result.add(Y);
            }
            else if ((pB2.getY() <= pA1.getY() && pB2.getY() >= pB1.getY()) || (pB2.getY() >= pA1.getY() && pB2.getY() <= pB1.getY()))
            {
                double Y = pB2.getY();
                result.add(X);
                result.add(Y);
            }
            else if ((pA2.getY() <= pA1.getY() && pA2.getY() >= pB1.getY()) || (pA2.getY() >= pA1.getY() && pA2.getY() <= pB1.getY()))
            {
                double Y = pA2.getY();
                result.add(X);
                result.add(Y);
            }
        }
    }
        return result;
    }
   
   
    //TODO
   
    //retourne le meilleur chemin possible en prenant pour param�tre l'identifiant de la salle o� on veut aller
    public ArrayList<WayPoint> getBestPath()
    {
        //stockera les points du meilleur chemin possible
        ArrayList<WayPoint> bestPath = new ArrayList<WayPoint>();
               
        //le chemin vers la destination voulue
        ArrayList<WayPoint> destinationPath = new ArrayList<WayPoint>(this.currentRoom.getPath());
               
        //le chemin de l'endroit actuel au point central
        ArrayList<WayPoint> actualPath = new ArrayList<WayPoint>(this.DestinationRoom.getPath());
               
        //On parcourt le chemin d'origine en sens inverse
        for(int i = actualPath.size()-1;i >= 0;i--)
        {
           
            if (i != 0)
            {
                //On stock le point actuel du chemin 1
                WayPoint pointPath1 = actualPath.get(i);
               
                //On recup�re le point pr�c�dent du chemin 1
                WayPoint nextPointPath1 = actualPath.get(i-1);
               
                // On ajoute le point actuel au bestPath
                bestPath.add(pointPath1);
               
               
                for(int j = destinationPath.size()-1;j >= 0;j--)
                {
                    if (j != 0)
                    {
                        //On stock le point actuel du chemin 2
                        //WayPoint pointPath2 = actualPath.get(j);
                        WayPoint pointPath2 = destinationPath.get(j);
                       
                        //On recup�re le point pr�c�dent du chemin 2
                        //WayPoint nextPointPath2 = actualPath.get(j-1);
                        WayPoint nextPointPath2 = destinationPath.get(j-1);
                       
                        //On r�cup�re le point d'intersection
                        ArrayList<Double> intersection = new ArrayList<Double>();
                        intersection = computeIntersection(pointPath1, nextPointPath1, nextPointPath2, pointPath2);
                       
                        // On v�rifie si une intersection a �t� trouv�e, sinon on ne fait rien et on va tester la prochaine droite du chemin 2 avec le for
                        if (intersection.size() != 0)
                        {
                            // On ajoute l'intersection au best path
                            WayPoint wp = new WayPoint(intersection.get(0), intersection.get(1));
                            // Si l'intersection est diff�rente du dernier WayPoint ins�r� dans le bestPath
                            if (!((wp.getX() == bestPath.get(bestPath.size()-1).getX()) && (wp.getY() == bestPath.get(bestPath.size()-1).getY())))
                            {
                                bestPath.add(wp);
                            }
                           
                            //Si l'intersection est un point du chemin 2
                            if (intersection.get(0) == pointPath2.getX() && intersection.get(1) == pointPath2.getY())
                            {
                                // on continue sur le chemin 2, à l'indice suivant l'intersection
                                addWayPoints(destinationPath, bestPath, j+1);
                            }
                            // on continue direct sur le chemin 2
                            else
                            {
                                addWayPoints(destinationPath, bestPath, j);
                            }
                           
                            // on retourne le bestPath
                            return bestPath;
                        }
                    }
                }   
            }
            //On est sur le dernier point du chemin 1 (point central), donc on ajoute tout le chemin 2 (qui contient déjà le point central)
            else
            {
                addWayPoints(destinationPath, bestPath, 0);
                System.out.println("Enregistre point central");
                return bestPath;
            }
        }
        // ON EST JAMAIS CENSE ARRIVER JUSQU'ICI !!!!!!!!!!!!!!!!!!!!!!
        System.out.println("Pas cense arriver la");
        return bestPath;
    }

}